#!/usr/bin/env python
# -*- coding: UTF-8 -*-
"""
@Project ：刘骏轩2025讯飞 
@File ：RED_GREEN.py
@Author ：ljx
@Date ：2025/8/7 下午9:43 
"""

import cv2
import rospy
import numpy as np
from GetCvPicture import PicGetter
from std_srvs.srv import Trigger, TriggerResponse


class Distinguish:

    def __init__(self):
        self.Green_UP = np.array([255, 80, 255])
        self.Green_LOW = np.array([0, 0, 0])
        self.RED_UP = np.array([255, 200, 255])
        self.RED_LOW = np.array([0, 255, 0])
        self.base_num = 2000

    def Dis_GREEN(self, img):
        lab_img = cv2.cvtColor(img, cv2.COLOR_BGR2Lab)

        mask = cv2.inRange(lab_img, self.Green_LOW, self.Green_UP)

        nums = np.count_nonzero(mask == 255)

        print(nums)

        return True if nums > self.base_num else False

    def Dis_RED(self, img):
        lab_img = cv2.cvtColor(img, cv2.COLOR_BGR2Lab)

        mask = cv2.inRange(lab_img, self.RED_LOW, self.RED_UP)

        nums = np.count_nonzero(mask == 255)

        print(nums)

        return True if nums > self.base_num else False


class DisService:
    def __init__(self):
        self.dis = Distinguish()
        self.service = None
        self.cap = PicGetter()

    def doRequest(self, req):
        img = self.cap.Get()

        while img is None:
            img = self.cap.Get()

        if self.dis.Dis_GREEN(img):
            return TriggerResponse(True, "green")
        else:
            return TriggerResponse(False, "red")

    def start(self):
        self.cap.start()
        rospy.loginfo("GREEN Service is ready")
        self.service = rospy.Service("GREENService", Trigger, self.doRequest)


if __name__ == '__main__':
    rospy.init_node('GREEN_NODE', anonymous=True)

    service = DisService()

    service.start()

    rospy.spin()
